/*
 * joystick.cc
 * Uses joystick to control robot
 */

#include <libplayerc++/playerc++.h>
#include "joystick.h"

using namespace PlayerCc;

double laser[361];

double robot_x;
double robot_y;
double robot_theta;

int main(int argc, char **argv){
    
    // connect to robot and relevant devices
    PlayerClient robot("localhost", 6665);
    Position2dProxy pp(&robot,0);
    LaserProxy lp(&robot,0);
	BlobfinderProxy bp(&robot,0);
    
    pp.SetMotorEnable (true);
    
    // this structure is maintained by the joystick reader
    memset( &cont, 0, sizeof(cont) );
    
    pthread_t dummy;
    
    if(!strlen(jsdev))
        jfd = open (DEFAULT_DEV, O_RDONLY);
    else
        jfd = open (jsdev, O_RDONLY);
    
    if(jfd < 1){
        perror("Failed to open joystick");
        fprintf(stderr, "Close application.");
    }
    else
        pthread_create(&dummy, NULL, &joystick_handler, (void*)&cont);
    //end of joystick read structure
    
    while( true ){
        
        //read data from devices
        robot.Read();
        
        //store laser info in array
        for(int i=0; i < 361; i++)
        	laser[i] = lp[i];
              
        robot_x = pp.GetXPos();
        robot_y = pp.GetYPos();
        robot_theta = pp.GetYaw();
        
        //if joystick has new info, update speed and turnrate
        if(cont.dirty){
            //button[10] handles the enabling/disabling of rear gear
            if(button[10] && cont.speed < 0)
                cont.speed=0;
            
            pp.SetSpeed(NORMALIZE_SPEED(cont.speed), NORMALIZE_TURN(cont.turnrate));
            
            cont.dirty = false; // we've handled the changes
        }     
        
        if(button[11]){
            
            std::cout << "GOODBYE!" << std::endl;
            
            robot.Stop(); 
            
            return(0);
            
        }
        
    }
    
    return(0);
    
}

//compile this program by inserting the following command in terminal while in its directory:
//g++ -o joystick `pkg-config --cflags playerc++` joystick.cc `pkg-config --libs playerc++`
